﻿using System;
using System.Collections.Generic;
using UnityEngine;

namespace SwanEngine.Astar
{
	public class Astar
	{
        private Grid grid;
        private Node endNode;
        private Node startNode;
        private int cut = 0;

        private List<int> openList;//开放列表
        private int openId = -1;//节点唯一id
        private int openCount = 0; //开放节点个数
        //private List<List<List<int>>> nodeList;//存放节点数据，包含是否在开放列表、关闭列表、NODEID
        private Dictionary<int, Dictionary<int, Dictionary<int, int>>> nodeList;
        private List<int> xList; //节点id对应x坐标
        private List<int> yList;//节点id对应y坐标
        private List<int> fatherList;//父节点坐标
        private List<int> costList;//移动估值
        private List<int> scoreList;//路径评分列表

        private const int NODE_OPEN = 0;
        private const int NODE_ID = 1;
        private const int NODE_CLOSE = 2;

        //横或者竖向移动一格的路径评分
        private const int COST_STRAIGHT = 10;
        //斜向移动一格的路径评分
        private const int COST_DIAGONAL = 14; 

        public Astar()
        {
            nodeList = new Dictionary<int, Dictionary<int, Dictionary<int, int>>>();
            xList = new List<int>();
            yList = new List<int>();
            fatherList = new List<int>();
            costList = new List<int>();
            scoreList = new List<int>();
            openList = new List<int>();
        }

        public void Init(MapDataVo vo)
        {
            this.reset();
            grid = new Grid(vo);
        }

        public List<Vector2> FindPath(Vector2 start, Vector2 end, int cut = 2, bool pathOptimize = true)
        {
            reset();
            startNode = checkNode(start, 20);
            endNode = checkNode(end, 20);

            Vector2 startPos = new Vector2(startNode.x, startNode.y);
            Vector2 endPos = new Vector2(endNode.x, endNode.y);

            this.cut = cut;
            if (startNode == null || endNode == null) return null;
            openNode(startNode.y, startNode.x, 0, 0, 0);
            int currId, currNodeX, currNodeY;
            List<Node> aroundNotes;
            int endX = endNode.x;
            int endZ = endNode.y;
            int cost;
            int score;
            int checkingId;

            while(openCount > 0)
            {
                //每次取出第一个开放列表中的id
                currId = openList[0];
                currNodeX = xList[currId];
                currNodeY = yList[currId];
                closeNode(currNodeY, currNodeX);
                if(currNodeX == endX && currNodeY == endZ)
                {
                    return getPath(startNode.x, startNode.y, currId, pathOptimize);
                }

                aroundNotes = getArounds(currNodeX, currNodeY);
                foreach(Node node in aroundNotes)
                {
                    cost = costList[currId] + ((node.x == currNodeX || node.y == currNodeY) ? COST_STRAIGHT : COST_DIAGONAL);
                    score = cost + (Math.Abs(endX - node.x) + Math.Abs(endZ - node.y)) * COST_STRAIGHT;
                    if(isOpen(node.y,node.x))
                    {
                        checkingId = nodeList[node.y][node.x][NODE_ID];
                        //如果新的g值比节点原来的G值小，修改F、G值，换父节点
                        if(cost < costList[checkingId])
                        {
                            costList[checkingId] = cost;
                            scoreList[checkingId] = score;
                            fatherList[checkingId] = currId;
                            aheadNode(getIndex(checkingId));
                        }
                    }
                    else
                    {
                        openNode(node.y, node.x, score, cost, currId);
                    }
                }

            }

            return null;
        }
        /**
		 *获取路径 
		 * @param startX int 起点x坐标
		 * @param startY int 起点y坐标
		 * @param currId int 终点id
		 * @return Array
		 * 
		 */
        private List<Vector2> getPath(int startX, int startY, int currId, bool pathOptimize)
        {
            List<Vector2> tempArr = new List<Vector2>();
            int nodeX = xList[currId];
            int nodeY = yList[currId];
            tempArr.Add(grid.getNode(nodeX,nodeY).pt);
            while(nodeX != startX || nodeY != startY)
            {
                currId = fatherList[currId];
                nodeX = xList[currId];
                nodeY = yList[currId];
                tempArr.Insert(0,grid.getNode(nodeX,nodeY).pt);
            }

            //路径优化
            if (pathOptimize)
            {
                List<Vector2> path = new List<Vector2>();
                path.Add(startNode.pt);
                int sx = startNode.x;
                int sy = startNode.y;
                int start = 0;
                int length = tempArr.Count;
                if (length > cut + 1)
                    length = length - cut;
                bool addEnd = false;
                for (int j = length - 1; j > start; j--)
                {
                    if (pathCheck(sx, sy, (int)tempArr[j].x, (int)tempArr[j].y) || j == start + 1)
                    {
                        path.Add(tempArr[j]);
                        if (j == length - 1)
                        {
                            addEnd = true;
                            break;
                        }
                        sx = (int)tempArr[j].x;
                        sy = (int)tempArr[j].y;
                        start = j;
                        j = length;
                    }
                }
                if (addEnd == false) path.Add(endNode.pt);
                if (path.Count > 1) path.RemoveAt(0);
                return path;
            }
            return tempArr;
        }

        public bool checkCanGo(float x1,float y1,float x2,float y2)
        {
            float size = 1;
            float dx = x2 - x1;
            float dy = y2 - y1;
            int len = (int)Math.Sqrt((double)((y2 - y1) * (y2 - y1) + (x2 - x1) * (x2 - x1)));
            float temp = 0;
            float goLength = size / 5;
            while (true)
            {
                if ((temp += goLength) > len)
                    return true;
                float rate = temp / len;
                float ny = rate * dy + y1;
                float nx = rate * dx + x1;
                if ((dx > 0 && nx > x2) || (dx < 0 && nx < x2))
                    nx = x2;
                if ((dy > 0 && ny > y2) || (dy < 0 && ny < y2))
                    ny = y2;
                if (grid.getNode((int)(nx / size), (int)(ny / size)) == null)
                    return false;
            }
            return true;
        }

        private bool pathCheck(float fx1, float fy1, float fx2, float fy2)
        {
            float size = 1;
            float halfSize = size / 2;

            float x1 = (fx1 * size + halfSize);
            float y1 = (fy1 * size + halfSize);
            float x2 = (fx2 * size + halfSize);
            float y2 = (fy2 * size + halfSize);

            float dx = x2 - x1;
            float dy = y2 - y1;
            int len = (int)Math.Sqrt((double)((y2 - y1) * (y2 - y1) + (x2 - x1) * (x2 - x1)));
            float temp = 0;
            float goLength = halfSize;
            while(true)
            {
                if ((temp += goLength) > len)
                    return true;
                float rate = temp / len;
                float ny = (rate * dy) + y1;
                float nx = (rate * dx) + x1;
                if ((dx > 0 && nx > x2) || (dx < 0 && nx < x2))
                    nx = x2;
                if ((dy > 0 && ny > y2) || (dy < 0 && ny < y2))
                    ny = y2;
                if (grid.getNode((int)(nx / size), (int)(ny / size)) == null)
                    return false;
            }

            return true;
        }

        private int getIndex(int checkingId)
        {
            return openList.IndexOf(checkingId) + 1;
        }

        public void setTileWalkable(int x, int y, bool value)
        {
            grid.setApplicationCheck(x,y, !value);
        }

        public bool getTileWalkable(int x, int y)
        {
            return grid.getNode(x,y) != null;
        }

        private List<Node> getArounds(int row, int column)
        {
            List<Node> arr = new List<Node>();
            Node node;
            int checkX, checkY;
            bool canDiagonal;

            //右
            checkX = column + 1;
            checkY = row;
            node = grid.getNode(checkY, checkX);
            bool canRight = node != null;
            if (canRight && !isClosed(checkX, checkY))
                arr.Add(node);

            //左
            checkX = column - 1;
            checkY = row;
            node = grid.getNode(checkY, checkX);
            bool canLeft = node != null;
            if (canLeft && !isClosed(checkX, checkY))
                arr.Add(node);

            //下
            checkX = column;
            checkY = row + 1;
            node = grid.getNode(checkY, checkX);
            bool canDown = node != null;
            if (canDown && !isClosed(checkX, checkY))
                arr.Add(node);

            //上
            checkX = column;
            checkY = row - 1;
            node = grid.getNode(checkY, checkX);
            bool canUp = node != null;
            if (canUp && !isClosed(checkX, checkY))
                arr.Add(node);

            //右下
            checkX = column + 1;
            checkY = row + 1;
            if((canRight || canDown) && !isClosed(checkX,checkY))
            {
                node = grid.getNode(checkY, checkX);
                canDiagonal = node != null;
                if (canDiagonal)
                    arr.Add(node);
            }

            //左下
            checkX = column - 1;
            if((canLeft || canDown) && !isClosed(checkX,checkY))
            {
                node = grid.getNode(checkY, checkX);
                canDiagonal = node != null;
                if (canDiagonal)
                    arr.Add(node);
            }

            //左上
            checkY = row - 1;
            if((canLeft || canUp) && !isClosed(checkX,checkY))
            {
                node = grid.getNode(checkY, checkX);
                canDiagonal = node != null;
                if (canDiagonal)
                    arr.Add(node);
            }

            //右上
            checkX = column + 1;
            if((canRight || canUp) && !isClosed(checkX,checkY))
            {
                node = grid.getNode(checkY, checkX);
                canDiagonal = node != null;
                if (canDiagonal)
                    arr.Add(node);
            }

            return arr;
        }

        /**
		 *判断节点是否在关闭列表 
		 * @param row int 节点所在行
		 * @param column int 节点所在列
		 * @return Boolean
		 * 
		 */	
        private bool isClosed(int row, int column)
        {
            if (nodeList.ContainsKey(row) == false) 
                return false;
            if (nodeList[row].ContainsKey(column) == false) 
                return false;

            return nodeList[row][column].ContainsKey(NODE_CLOSE) && nodeList[row][column][NODE_CLOSE] == 1;
        }

        /**
		 * @private
		 * 判断某节点是否在开放列表
		 */	
        private bool isOpen(int row, int column)
        {
            if (nodeList.ContainsKey(row) == false)
                return false;
            if (nodeList[row].ContainsKey(column) == false)
                return false;

            return nodeList[row][column].ContainsKey(NODE_OPEN) && nodeList[row][column][NODE_OPEN] == 1;
        }

        private void reset()
        {
            openId = -1;
            openCount = 0;
            xList.Clear();
            yList.Clear();
            nodeList.Clear();
            fatherList.Clear();
            costList.Clear();
            scoreList.Clear();
            openList.Clear();
        }

        /**
		 *关闭某个节点 
		 * @param nodeId int 节点id
		 * 
		 */
        private void closeNode(int row, int column)
        {
            openCount--;
            nodeList[row][column][NODE_OPEN] = 0;
            nodeList[row][column][NODE_CLOSE] = 1;
            if(openCount <= 0)
            {
                openCount = 0;
                openList.Clear();
                return;
            }
            openList[0] = openList[openList.Count - 1];
            openList.RemoveAt(openList.Count - 1);
            backNode();
        }

        /**
		 *将第一个节点按二叉树往下沉 
		 * 
		 */

        private void backNode()
        {
            int index = 1;
            int tmp, change, son;
            while(true)
            {
                tmp = index;
                son = tmp << 1;
                if(son < openCount)//加入有子节点
                {
                    if (getScore(index) > getScore(son))
                        index = son;
                    son++;
                    if(son < openCount)
                    {
                        if (getScore(index) > getScore(son))
                            index = son;
                    }
                }
                if(tmp == index)
                    break;
                else
                {
                    change = openList[tmp - 1];
                    openList[tmp - 1] = openList[index - 1];
                    openList[index - 1] = change;
                }
            }
        }

        /**
		 * 打开节点，放入开放列表 
		 * @param row int 节点所在行
		 * @param column int 节点所在列
		 * @param score int 路径评分
		 * @param cost int 移动估值
		 * @param fatherNode 父节点id
		 * 
		 */
        private void openNode(int row,int column,int score,int cost,int fatherNode)
        {
            openId++;
            openCount++;
            openList.Add(openId);
            xList.Add(column);
            yList.Add(row);
            if (nodeList.ContainsKey(row) == false)
                nodeList.Add(row, new Dictionary<int, Dictionary<int, int>>());
            nodeList[row][column] = new Dictionary<int, int>();
            nodeList[row][column][NODE_OPEN] = 1;
            nodeList[row][column][NODE_ID] = openId;
            fatherList.Add(fatherNode);
            scoreList.Add(score);
            costList.Add(cost);
            aheadNode(openCount);

        }
        /**
		 *将节点按二叉法向前移动 
		 * @param index int 当前节点的索引位+1，即当前移动节点排在数组中第几个， 从1开始计数，第一个元素是1
		 * 
		 */	
        private void aheadNode(int index)
        {
            int change, fatherId;
            int scoreIndex = getScore(index);
            while(index > 1)
            {
                fatherId = index >> 1;
                if (scoreIndex < getScore(fatherId))
                {
                    change = openList[index - 1];
                    openList[index - 1] = openList[fatherId - 1];
                    openList[fatherId - 1] = change;
                    index = fatherId;
                }
                else
                    break;
            }
        }
        /**
		 *获取节点的评分 
		 * @param index int 表示第几个节点  从1开始
		 * @return int
		 * 
		 */	
        private int getScore(int index)
        {
            return scoreList[openList[index - 1]];
        }

        public Node getArround(int x, int y)
        {
            Vector2 temp = new Vector2(x, y);
            for (int i = 1; i <= 3; i++)
            {
                List<Node> outLine = getOutLine(temp, i);
                if (outLine.Count > 0)
                {
                    int index = UnityEngine.Random.Range(0, outLine.Count - 1);
                    return outLine[index];
                }
            }
            return null;
        }
        
        public Node checkNode(Vector2 endPt, int cycle = int.MaxValue)
        {
            Node node = grid.getNode((int)endPt.x, (int)endPt.y);
            if(node != null)
            {
                return node;
            }
            else
            {
                for(int i = 1; i <= cycle; i++)
                {
                    List<Node> outLine = getOutLine(endPt, i);
                    if (outLine.Count > 0)
                        return outLine[0];
                }
            }
            return null;
        }
        private List<Node> getOutLine(Vector2 pt, int cycle = 1)
        {
            List<Node> result = new List<Node>();
            for(int i =(int)( pt.x -cycle); i <= pt.x + cycle; i++)
                for(int j = (int)(pt.y - cycle);j <= pt.y + cycle; j++)
                {
                    Node temp = grid.getNode(i, j);
                    if(temp != null)
                    {
                        int dx = Math.Abs((int)(temp.x - pt.x));
                        int dy = Math.Abs((int)(temp.y - pt.y));
                        if (dx == cycle || dy == cycle)
                            result.Add(temp);
                    }
                }

            return result;
        }
	}
}
